% FastSLAM 1.0 仿真主程序（激光雷达版）

clear all;
close all;
clc;

% 添加路径
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'common'));
addpath(fullfile(fileparts(mfilename('fullpath')), '..', 'occupancy_grid'));

% 轨迹选择
fprintf('========================================\n');
fprintf('  FastSLAM 轨迹模式选择\n');
fprintf('========================================\n');
fprintf('1. 固定路径点轨迹（可重复，适合对比）\n');
fprintf('2. 动态避障轨迹（随机探索）\n');
fprintf('========================================\n');
choice = input('请选择轨迹模式 [1/2] (默认1): ', 's');
if isempty(choice)
    choice = '1';
end

%% 加载统一配置
config = get_slam_config();

% 应用选择
if choice == '1'
    config.use_fixed_trajectory = true;
    fprintf('使用固定路径点轨迹\n\n');
elseif choice == '2'
    config.use_fixed_trajectory = false;
    fprintf('使用动态避障轨迹\n\n');
else
    fprintf('无效选择，使用默认（固定路径点）\n\n');
    config.use_fixed_trajectory = true;
end

% 解包常用参数
dt = config.dt;
total_time = config.total_time;
steps = config.steps;
Q = config.Q;
R = config.R;
laser_range = config.laser_range;
laser_fov = config.laser_fov;
num_beams = config.num_beams;
laser_noise = config.laser_noise;

% FastSLAM特有参数
N_particles = config.fastslam.N_particles;
resample_threshold = config.fastslam.resample_threshold;

% 统一环境地图
[~, obstacles] = generate_landmarks();
n_obstacles = size(obstacles, 1);

%% 初始化状态
if config.use_fixed_trajectory
    true_state = config.initial_state;
else
    % 动态模式：使用随机初始方向
    initial_theta = 0;  % 固定初始方向：水平向右
    true_state = [config.initial_state(1); config.initial_state(2); initial_theta];
end

% 轨迹配置
USE_FIXED_TRAJECTORY = config.use_fixed_trajectory;

if USE_FIXED_TRAJECTORY
    fprintf('FastSLAM + 激光雷达 [固定路径点轨迹, 粒子:%d, 障碍物:%d, 时长:%.0fs]\n', N_particles, n_obstacles, total_time);
    controls = generate_waypoint_trajectory(config.waypoints, dt, total_time, true_state);
else
    fprintf('FastSLAM + 激光雷达 [随机轨迹, 粒子:%d, 障碍物:%d, 时长:%.0fs]\n', N_particles, n_obstacles, total_time);
    v_base = config.v_base;
    omega_base = config.omega_base;
end

%% 初始化粒子（使用正确的起始位置）
particles = initialize_particles(N_particles, true_state, [0.1; 0.1; 0.02]);

% 数据存储
true_trajectory = zeros(3, steps);
estimated_trajectory = zeros(3, steps);

% 运动控制状态（仅在非固定轨迹模式下使用）
if ~USE_FIXED_TRAJECTORY
    motion_state = struct();
    motion_state.boundary_avoiding = false;
    motion_state.obstacle_avoiding = false;
    motion_state.avoidance_timer = 0;
    motion_state.angle_diff = 0;
    motion_state.v = config.v_base;
    motion_state.omega = config.omega_base;
end

%% 主循环
figure('Position', config.display.figure_position);

for t = 1:steps
    %% 获取控制输入
    if USE_FIXED_TRAJECTORY
        control = controls(:, t);
    else
        [v, omega, motion_state] = safe_motion_controller(true_state, obstacles, motion_state, v_base, omega_base, dt);
        control = [v; omega];
    end
    
    %% 真实运动
    true_state = motion_model(true_state, control, dt, Q);
    
    % 安全检查
    [still_collision, ~] = check_collision(true_state, obstacles, 0.5);
    if still_collision && t > 1
        true_state = true_trajectory(:, t-1);
    end
    
    true_trajectory(:, t) = true_state;
    
    %% 激光观测
    [observations, observed_ids] = observation_model_laser(true_state, obstacles, ...
                                    laser_range, laser_fov, num_beams, laser_noise);
    
    %% FastSLAM预测
    particles = particle_predict(particles, control, dt, Q);
    
    %% FastSLAM更新
    particles = particle_update(particles, observations, observed_ids, R);
    
    %% 重采样
    N_eff = 1 / sum([particles.weight].^2);
    if N_eff < N_particles * resample_threshold
        particles = resample_particles(particles);
    end
    
    %% 估计状态
    estimated_state = estimate_state(particles);
    estimated_trajectory(:, t) = estimated_state;
    
    %% 可视化
    if mod(t, config.display.update_interval) == 0
        plot_fastslam_laser(true_state, particles, true_trajectory(:, 1:t), ...
                           estimated_trajectory(:, 1:t), obstacles, t*dt);
        drawnow;
    end
end

%% 结果统计
fprintf('\n仿真完成\n');

% 统计发现的路标
all_landmarks = [];
for i = 1:length(particles)
    if particles(i).weight > 0.01
        all_landmarks = [all_landmarks; particles(i).landmark_ids];
    end
end
unique_landmarks = unique(all_landmarks);
fprintf('发现特征: %d\n', length(unique_landmarks));

position_error = sqrt(sum((true_trajectory(1:2, :) - estimated_trajectory(1:2, :)).^2, 1));
fprintf('平均误差: %.3f m\n', mean(position_error));
fprintf('最大误差: %.3f m\n', max(position_error));

save('fastslam_result.mat', 'true_trajectory', 'estimated_trajectory', 'position_error', 'particles');
